using System;
using System.Collections;
using System.Collections.Generic;
using System.Runtime.CompilerServices;

namespace LightCAD.MathLib
{
    public sealed class Box3
    {
        #region scope properties or methods
        //private static Vector3[] _points = new Vector3[]
        //{
        //    new Vector3(),
        //    new Vector3(),
        //    new Vector3(),
        //    new Vector3(),
        //    new Vector3(),
        //    new Vector3(),
        //    new Vector3(),
        //    new Vector3()
        //};
        //private static Vector3 _vector = new Vector3();
        //private static Box3 _box = new Box3();
        //private static Vector3 _v0 = new Vector3();
        //private static Vector3 _v1 = new Vector3();
        //private static Vector3 _v2 = new Vector3();
        //private static Vector3 _f0 = new Vector3();
        //private static Vector3 _f1 = new Vector3();
        //private static Vector3 _f2 = new Vector3();
        //private static Vector3 _center = new Vector3();
        //private static Vector3 _extents = new Vector3();
        //private static Vector3 _triangleNormal = new Vector3();
        //private static Vector3 _testAxis = new Vector3();
        public static Box3Context GetContext()
        {
            return ThreadContext.GetCurrThreadContext().Box3Ctx;
        }
        private static bool SatForAxes(double[] axes, Vector3 v0, Vector3 v1, Vector3 v2, Vector3 extents)
        {
            var _testAxis = GetContext()._testAxis;
            for (int i = 0, j = axes.Length - 3; i <= j; i += 3)
            {

                _testAxis.FromArray(axes, i);
                // project the aabb onto the separating axis

                // project all 3 vertices of the triangle onto the separating axis
                double r = extents.X * Math.Abs(_testAxis.X) + extents.Y * Math.Abs(_testAxis.Y) + extents.Z * Math.Abs(_testAxis.Z);

                double p0 = v0.Dot(_testAxis);
                double p1 = v1.Dot(_testAxis);
                double p2 = v2.Dot(_testAxis);


                // actual test, basically see if either of the most extreme of the triangle points intersects r
                if (Math.Max(-MathEx.Max(p0, p1, p2), MathEx.Min(p0, p1, p2)) > r)
                {

                    // points of the projected triangle are outside the projected half-length of the aabb
                    // the axis is separating and we can exit
                    return false;

                }

            }

            return true;

        }
        #endregion

        #region Properties

        public Vector3 Min;
        public Vector3 Max;

        #endregion

        #region constructor
        public Box3(Vector3 min = null, Vector3 max = null)
        {
            if (min == null) min = new Vector3(double.PositiveInfinity, double.PositiveInfinity, double.PositiveInfinity);
            if (max == null) max = new Vector3(double.NegativeInfinity, double.NegativeInfinity, double.NegativeInfinity);

            this.Min = min;
            this.Max = max;
        }
        #endregion

        #region methods
        [MethodImpl((MethodImplOptions)768)]
        public Box3 Set(Vector3 min, Vector3 max)
        {
            this.Min.Copy(min);
            this.Max.Copy(max);
            return this;
        }
        [MethodImpl((MethodImplOptions)768)]
        public Box3 SetFromArray(double[] array)
        {
            var _vector = GetContext()._vector;
            this.MakeEmpty();

            for (int i = 0, il = array.Length; i < il; i += 3)
            {
                this.ExpandByPoint(_vector.FromArray(array, i));
            }
            return this;

        }
        [MethodImpl((MethodImplOptions)768)]
        public Box3 SetFromPoints(IList<Vector3> points)
        {
            this.MakeEmpty();
            for (int i = 0, il = points.Count; i < il; i++)
            {
                this.ExpandByPoint(points[i]);
            }
            return this;
        }
        [MethodImpl((MethodImplOptions)768)]
        public Box3 SetFromCenterAndSize(Vector3 center, Vector3 size)
        {
            var _vector = GetContext()._vector;
            Vector3 halfSize = _vector.Copy(size).MulScalar(0.5);
            this.Min.Copy(center).Sub(halfSize);
            this.Max.Copy(center).Add(halfSize);
            return this;
        }

        [MethodImpl((MethodImplOptions)768)]
        public Box3 Clone()
        {
            return new Box3().Copy(this);
        }
        [MethodImpl((MethodImplOptions)768)]
        public Box3 Copy(Box3 box)
        {
            this.Min.Copy(box.Min);
            this.Max.Copy(box.Max);
            return this;
        }
        [MethodImpl((MethodImplOptions)768)]
        public Box3 MakeEmpty()
        {
            this.Min.X = this.Min.Y = this.Min.Z = double.PositiveInfinity;
            this.Max.X = this.Max.Y = this.Max.Z = double.NegativeInfinity;
            return this;
        }
        [MethodImpl((MethodImplOptions)768)]
        public bool IsEmpty()
        {
            // this is a more robust check for empty than ( volume <= 0 ) because volume can get positive with two negative axes
            return (this.Max.X < this.Min.X) || (this.Max.Y < this.Min.Y) || (this.Max.Z < this.Min.Z);
        }
        [MethodImpl((MethodImplOptions)768)]
        public Vector3 GetCenter(Vector3 target = null)
        {
            target = target ?? new Vector3();
            return this.IsEmpty() ? target.Set(0, 0, 0) : target.AddVectors(this.Min, this.Max).MulScalar(0.5);
        }
        [MethodImpl((MethodImplOptions)768)]
        public Vector3 GetSize(Vector3 target = null)
        {
            target = target ?? new Vector3();
            return this.IsEmpty() ? target.Set(0, 0, 0) : target.SubVectors(this.Max, this.Min);
        }
        [MethodImpl((MethodImplOptions)768)]
        public Box3 ExpandByPoint(Vector3 point)
        {
            this.Min.Min(point);
            this.Max.Max(point);
            return this;
        }
        [MethodImpl((MethodImplOptions)768)]
        public Box3 ExpandByVector(Vector3 vector)
        {
            this.Min.Sub(vector);
            this.Max.Add(vector);
            return this;
        }
        [MethodImpl((MethodImplOptions)768)]
        public Box3 ExpandByScalar(double scalar)
        {
            this.Min.AddScalar(-scalar);
            this.Max.AddScalar(scalar);
            return this;
        }
        [MethodImpl((MethodImplOptions)768)]
        public Box3 ExpandByCenter(Vector3 scale)
        {
            var size = GetSize(new Vector3()).Mul(scale);
            var center = GetCenter(new Vector3());
            this.Min.Set(center.X - size.X / 2, center.Y - size.Y / 2, center.Z - size.Z / 2);
            this.Max.Set(center.X + size.X / 2, center.Y + size.Y / 2, center.Z + size.Z / 2);
            return this;
        }
        [MethodImpl((MethodImplOptions)768)]
        public bool ContainsPoint(Vector3 point)
        {
            return point.X < this.Min.X || point.X > this.Max.X ||
                point.Y < this.Min.Y || point.Y > this.Max.Y ||
                point.Z < this.Min.Z || point.Z > this.Max.Z ? false : true;
        }
        [MethodImpl((MethodImplOptions)768)]
        public bool ContainsBox(Box3 box)
        {
            return this.Min.X <= box.Min.X && box.Max.X <= this.Max.X &&
                this.Min.Y <= box.Min.Y && box.Max.Y <= this.Max.Y &&
                this.Min.Z <= box.Min.Z && box.Max.Z <= this.Max.Z;
        }
        [MethodImpl((MethodImplOptions)768)]
        public Vector3 GetParameter(Vector3 point, Vector3 target)
        {
            // This can potentially have a divide by zero if the box
            // has a size dimension of 0.
            return target.Set(
                (point.X - this.Min.X) / (this.Max.X - this.Min.X),
                (point.Y - this.Min.Y) / (this.Max.Y - this.Min.Y),
                (point.Z - this.Min.Z) / (this.Max.Z - this.Min.Z)
            );
        }
        [MethodImpl((MethodImplOptions)768)]
        public bool IntersectsBox(Box3 box)
        {
            // using 6 splitting planes to rule out intersections.
            return box.Max.X < this.Min.X || box.Min.X > this.Max.X ||
                box.Max.Y < this.Min.Y || box.Min.Y > this.Max.Y ||
                box.Max.Z < this.Min.Z || box.Min.Z > this.Max.Z ? false : true;
        }
        [MethodImpl((MethodImplOptions)768)]
        public bool IntersectsSphere(Sphere sphere)
        {
            var ctx = GetContext();
            var _vector = ctx._vector;
            // Find the point on the AABB closest to the sphere center.
            this.ClampPoint(sphere.Center, _vector);
            // If that point is inside the sphere, the AABB and sphere intersect.
            return _vector.DistanceToSquared(sphere.Center) <= (sphere.Radius * sphere.Radius);
        }
        public bool IntersectsPlane(Plane plane)
        {
            // We compute the minimum and maximum dot product values. If those values
            // are on the same side (back or front) of the plane, then there is no intersection.
            double min, max;
            if (plane.Normal.X > 0)
            {
                min = plane.Normal.X * this.Min.X;
                max = plane.Normal.X * this.Max.X;
            }
            else
            {
                min = plane.Normal.X * this.Max.X;
                max = plane.Normal.X * this.Min.X;
            }
            if (plane.Normal.Y > 0)
            {
                min += plane.Normal.Y * this.Min.Y;
                max += plane.Normal.Y * this.Max.Y;
            }
            else
            {
                min += plane.Normal.Y * this.Max.Y;
                max += plane.Normal.Y * this.Min.Y;
            }
            if (plane.Normal.Z > 0)
            {
                min += plane.Normal.Z * this.Min.Z;
                max += plane.Normal.Z * this.Max.Z;
            }
            else
            {
                min += plane.Normal.Z * this.Max.Z;
                max += plane.Normal.Z * this.Min.Z;
            }
            return (min <= -plane.Constant && max >= -plane.Constant);
        }
        public bool IntersectsTriangle(Triangle triangle)
        {
            if (this.IsEmpty())
            {
                return false;
            }
            var ctx = GetContext();
            var _center = ctx._center;
            var _extents = ctx._extents;
            var _v0 = ctx._v0;
            var _v1 = ctx._v1;
            var _v2 = ctx._v2;
            var _f0 = ctx._f0;
            var _f1 = ctx._f1;
            var _f2 = ctx._f2;
            var _triangleNormal = ctx._triangleNormal;
            // compute box center and extents
            this.GetCenter(_center);
            _extents.SubVectors(this.Max, _center);
            // translate triangle to aabb origin
            _v0.SubVectors(triangle.A, _center);
            _v1.SubVectors(triangle.B, _center);
            _v2.SubVectors(triangle.C, _center);
            // compute edge vectors for triangle
            _f0.SubVectors(_v1, _v0);
            _f1.SubVectors(_v2, _v1);
            _f2.SubVectors(_v0, _v2);
            // test against axes that are given by cross product combinations of the edges of the triangle and the edges of the aabb
            // make an axis testing of each of the 3 sides of the aabb against each of the 3 sides of the triangle = 9 axis of separation
            // axis_ij = u_i x f_j (u0, u1, u2 = face normals of aabb = x,y,z axes vectors since aabb is axis aligned)
            var axes = new double[] {
                    0, -_f0.Z, _f0.Y, 0, -_f1.Z, _f1.Y, 0, -_f2.Z, _f2.Y,
                    _f0.Z, 0, -_f0.X, _f1.Z, 0, -_f1.X, _f2.Z, 0, -_f2.X,
                    -_f0.Y, _f0.X, 0, -_f1.Y, _f1.X, 0, -_f2.Y, _f2.X, 0
                };
            if (!SatForAxes(axes, _v0, _v1, _v2, _extents))
            {
                return false;
            }
            // test 3 face normals from the aabb
            axes = new double[] { 1, 0, 0, 0, 1, 0, 0, 0, 1 };
            if (!SatForAxes(axes, _v0, _v1, _v2, _extents))
            {
                return false;
            }
            // finally testing the face normal of the triangle
            // use already existing triangle edge vectors here
            _triangleNormal.CrossVectors(_f0, _f1);
            axes = new double[] { _triangleNormal.X, _triangleNormal.Y, _triangleNormal.Z };
            return SatForAxes(axes, _v0, _v1, _v2, _extents);
        }

        [MethodImpl((MethodImplOptions)768)]
        public Vector3 ClampPoint(Vector3 point, Vector3 target)
        {
            return target.Copy(point).Clamp(this.Min, this.Max);
        }
        [MethodImpl((MethodImplOptions)768)]
        public double DistanceToPoint(Vector3 point)
        {
            var _vector = GetContext()._vector;
            return this.ClampPoint(point, _vector).DistanceTo(point);
        }
        [MethodImpl((MethodImplOptions)768)]
        public Sphere GetBoundingSphere(Sphere target)
        {
            if (this.IsEmpty())
            {
                target.MakeEmpty();
            }
            else
            {
                var _vector = GetContext()._vector;
                this.GetCenter(target.Center);
                target.Radius = this.GetSize(_vector).Length() * 0.5;
            }
            return target;
        }
        [MethodImpl((MethodImplOptions)768)]
        public Box3 Intersect(Box3 box)
        {
            this.Min.Max(box.Min);
            this.Max.Min(box.Max);
            // ensure that if there is no overlap, the result is fully empty, not slightly empty with non-inf/+inf values that will cause subsequence intersects to erroneously return valid values.
            if (this.IsEmpty()) this.MakeEmpty();
            return this;
        }
        [MethodImpl((MethodImplOptions)768)]
        public Box3 Union(Box3 box)
        {
            this.Min.Min(box.Min);
            this.Max.Max(box.Max);
            return this;
        }
        [MethodImpl((MethodImplOptions)768)]
        public Box3 ApplyMatrix4(Matrix4 matrix)
        {
            // transform of empty box is an empty box.
            if (this.IsEmpty()) return this;
            var _points = GetContext()._points;
            // NOTE: I am using a binary pattern to specify all 2^3 combinations below
            _points[0].Set(this.Min.X, this.Min.Y, this.Min.Z).ApplyMatrix4(matrix); // 000
            _points[1].Set(this.Min.X, this.Min.Y, this.Max.Z).ApplyMatrix4(matrix); // 001
            _points[2].Set(this.Min.X, this.Max.Y, this.Min.Z).ApplyMatrix4(matrix); // 010
            _points[3].Set(this.Min.X, this.Max.Y, this.Max.Z).ApplyMatrix4(matrix); // 011
            _points[4].Set(this.Max.X, this.Min.Y, this.Min.Z).ApplyMatrix4(matrix); // 100
            _points[5].Set(this.Max.X, this.Min.Y, this.Max.Z).ApplyMatrix4(matrix); // 101
            _points[6].Set(this.Max.X, this.Max.Y, this.Min.Z).ApplyMatrix4(matrix); // 110
            _points[7].Set(this.Max.X, this.Max.Y, this.Max.Z).ApplyMatrix4(matrix); // 111
            this.SetFromPoints(_points);
            return this;
        }
        [MethodImpl((MethodImplOptions)768)]
        public Box3 Translate(Vector3 offset)
        {
            this.Min.Add(offset);
            this.Max.Add(offset);
            return this;
        }
        [MethodImpl((MethodImplOptions)768)]
        public bool Equals(Box3 box)
        {
            return box.Min.Equals(this.Min) && box.Max.Equals(this.Max);
        }
        #endregion

    }
    public sealed class Box3Context
    {
        public readonly Vector3[] _points = new Vector3[]
       {
                new Vector3(),
                new Vector3(),
                new Vector3(),
                new Vector3(),
                new Vector3(),
                new Vector3(),
                new Vector3(),
                new Vector3()
       };
        public readonly Vector3 _vector = new Vector3();
        public readonly Box3 _box = new Box3();
        public readonly Vector3 _v0 = new Vector3();
        public readonly Vector3 _v1 = new Vector3();
        public readonly Vector3 _v2 = new Vector3();
        public readonly Vector3 _f0 = new Vector3();
        public readonly Vector3 _f1 = new Vector3();
        public readonly Vector3 _f2 = new Vector3();
        public readonly Vector3 _center = new Vector3();
        public readonly Vector3 _extents = new Vector3();
        public readonly Vector3 _triangleNormal = new Vector3();
        public readonly Vector3 _testAxis = new Vector3();
    }
}
